#include "stdafx.h"

#include "LocatorPoseMex.h"

void runCircleFitGeometric(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
	if (nrhs != 4) mexErrMsgTxt("CircleFitGeometric needs 3 arguments.");
	if (mxGetM(prhs[1]) != 2) mexErrMsgTxt("Y needs to be 2 rows, n colums matrix.");
	if (mxGetM(prhs[2]) * mxGetN(prhs[2]) != 1) mexErrMsgTxt("r0 must be a scalar.");
	if ( (mxGetM(prhs[3]) != 2) || (mxGetN(prhs[3]) != 1) ) mexErrMsgTxt("center0 must be a 2x1 vector.");
	
	// 2. Argument in Matrix Y kopieren
	/*arma::mat Y(1,1);
	const double *pY_orig = Y.memptr();
	convertMatlabToArma(prhs[1], Y);*/
	arma::mat Y = convertMatlabToArma(prhs[1]);
	
	// 3. Argument in double alpha0 kopieren
	double r0 = 0;
	memcpy(&r0, mxGetPr(prhs[2]), sizeof(double));
	
	// 4. Argument in Matrix center0 kopieren
	//arma::mat center0(1,1);
	//const double *pcenter0_orig = center0.memptr();
	//convertMatlabToArma(prhs[3], center0);
	arma::colvec center0 = convertMatlabToArma(prhs[3]);
	
	
	double r;
	arma::colvec phi;
	arma::colvec center;
	arma::colvec s_center;
	bool success;
	
	try {
		OTPF::Fit::Circle::fitGeometric(Y, center0, r0, r, phi, center, s_center, success);
	} catch(OTPF::Error::Matrix_size e) {
		mexErrMsgTxt(e.p);
	} catch(OTPF::Error::General e) {
		mexErrMsgTxt(e.p);
	} catch(...) {
		mexErrMsgTxt("Unknown error in OTPF::Circle::FitGeometric");
	}
	
	// Ergebnisse in Matrizen konvertieren und in den Ausgabearray schreiben
	
	plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);	// r
	memcpy(mxGetPr(plhs[0]), &r, sizeof(double));
	plhs[1] = convertArmaToMatlab(phi);				// phi
	plhs[2] = convertArmaToMatlab(center);			// center
	
	plhs[3] = convertArmaToMatlab(s_center);		// s_center
	
	double succ = (double) success;	// success muss vorher in double konvertiert werden
	plhs[4] = mxCreateDoubleMatrix(1, 1, mxREAL);	// successful?
	memcpy(mxGetPr(plhs[4]), &succ, sizeof(double));
	
	
	//freeArma(Y, pY_orig);
	//freeArma(center0, pcenter0_orig);
	
	return;
};
